#include "robertStep.h"
#include "driver/mcpwm.h"
#define STEP_INTERVAL 15
// 定义步伐数组时应定义为steps[m][n]形式，而不能定义为steps[][n]这种不完全类型，因为在行进函数中，要通过sizeof函数取步数m，定义为不完全类型编译会报错
float ForwardSteps[15][9] = {
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(45), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(90), 100},
    {DEG2DUTY(45), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(45), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(135), DEG2DUTY(90), DEG2DUTY(45), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(90), 100},

    {DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(90), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(135), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},

    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(45), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(90), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
};

float BackwardSteps[15][9] = {
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(90), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(45), DEG2DUTY(70), 100},
    {DEG2DUTY(45), DEG2DUTY(70), DEG2DUTY(135), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(45), DEG2DUTY(70), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(90), 100},

    {DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(90), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},

    {DEG2DUTY(45), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(45), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(45), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
};
float MoveLeftSteps[11][9] = {
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(45), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(90), 100},
    {DEG2DUTY(135), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(90), 100},
    {DEG2DUTY(135), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(135), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(135), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(90), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
};
float MoveRightSteps[11][9] = {
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(45), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(90), 100},
    {DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(90), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(135), DEG2DUTY(70), 100},
    {DEG2DUTY(135), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(135), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(135), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
};
float TurnLeftSteps[8][9] = {
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(90), 100},
    {DEG2DUTY(135), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(90), 100},
    {DEG2DUTY(135), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(70), 100},
    {DEG2DUTY(135), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(135), DEG2DUTY(70), 100},
    {DEG2DUTY(135), DEG2DUTY(70), DEG2DUTY(135), DEG2DUTY(90), DEG2DUTY(135), DEG2DUTY(90), DEG2DUTY(135), DEG2DUTY(70), 100},
    {DEG2DUTY(135), DEG2DUTY(70), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
};

float TurnRightSteps[8][9] = {
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(45), DEG2DUTY(90), DEG2DUTY(45), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(90), 100},
    {DEG2DUTY(45), DEG2DUTY(90), DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(90), 100},
    {DEG2DUTY(45), DEG2DUTY(70), DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(70), 100},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 100},
};
float LieSteps[1][9] = {
    {DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), 500},
};
float HelloSteps[4][9] = {
    {DEG2DUTY(90), DEG2DUTY(120), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(60), DEG2DUTY(90), DEG2DUTY(70), 200},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 200},
    {DEG2DUTY(90), DEG2DUTY(120), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(60), DEG2DUTY(90), DEG2DUTY(70), 200},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 200},
};
float FightSteps[11][9] = {
    {DEG2DUTY(90), DEG2DUTY(120), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(60), DEG2DUTY(90), DEG2DUTY(70), 200},
    {DEG2DUTY(70), DEG2DUTY(120), DEG2DUTY(70), DEG2DUTY(110), DEG2DUTY(70), DEG2DUTY(60), DEG2DUTY(70), DEG2DUTY(70), 200},
    {DEG2DUTY(110), DEG2DUTY(120), DEG2DUTY(110), DEG2DUTY(110), DEG2DUTY(110), DEG2DUTY(60), DEG2DUTY(110), DEG2DUTY(70), 200},
    {DEG2DUTY(70), DEG2DUTY(120), DEG2DUTY(70), DEG2DUTY(110), DEG2DUTY(70), DEG2DUTY(60), DEG2DUTY(70), DEG2DUTY(70), 200},
    {DEG2DUTY(110), DEG2DUTY(120), DEG2DUTY(110), DEG2DUTY(110), DEG2DUTY(110), DEG2DUTY(60), DEG2DUTY(110), DEG2DUTY(70), 200},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), 200},
    {DEG2DUTY(70), DEG2DUTY(70), DEG2DUTY(70), DEG2DUTY(70), DEG2DUTY(70), DEG2DUTY(110), DEG2DUTY(70), DEG2DUTY(110), 200},
    {DEG2DUTY(110), DEG2DUTY(70), DEG2DUTY(110), DEG2DUTY(70), DEG2DUTY(110), DEG2DUTY(110), DEG2DUTY(110), DEG2DUTY(110), 200},
    {DEG2DUTY(70), DEG2DUTY(70), DEG2DUTY(70), DEG2DUTY(70), DEG2DUTY(70), DEG2DUTY(110), DEG2DUTY(70), DEG2DUTY(110), 200},
    {DEG2DUTY(110), DEG2DUTY(70), DEG2DUTY(110), DEG2DUTY(70), DEG2DUTY(110), DEG2DUTY(110), DEG2DUTY(110), DEG2DUTY(110), 200},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), 200}
};
float PushUpSteps[11][9] = {
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 300},
    {DEG2DUTY(90), DEG2DUTY(100), DEG2DUTY(90), DEG2DUTY(80), DEG2DUTY(90), DEG2DUTY(80), DEG2DUTY(90), DEG2DUTY(100), 400},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 500},
    {DEG2DUTY(90), DEG2DUTY(100), DEG2DUTY(90), DEG2DUTY(80), DEG2DUTY(90), DEG2DUTY(80), DEG2DUTY(90), DEG2DUTY(100), 600},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 700},
    {DEG2DUTY(90), DEG2DUTY(100), DEG2DUTY(90), DEG2DUTY(80), DEG2DUTY(90), DEG2DUTY(80), DEG2DUTY(90), DEG2DUTY(100), 1300},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 1800},
    {DEG2DUTY(90), DEG2DUTY(135), DEG2DUTY(90), DEG2DUTY(45), DEG2DUTY(90), DEG2DUTY(45), DEG2DUTY(90), DEG2DUTY(135), 200},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(45), DEG2DUTY(90), DEG2DUTY(60), DEG2DUTY(90), DEG2DUTY(135), 500},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(45), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(135), 500},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 500}
};
float CrabDanceSteps[2][9] = {
    {DEG2DUTY(90), DEG2DUTY(30), DEG2DUTY(90), DEG2DUTY(150), DEG2DUTY(90), DEG2DUTY(150), DEG2DUTY(90), DEG2DUTY(30), 200},
    {DEG2DUTY(45), DEG2DUTY(30), DEG2DUTY(135), DEG2DUTY(150), DEG2DUTY(135), DEG2DUTY(150), DEG2DUTY(45), DEG2DUTY(30), 200},
};
float Dance1Steps[10][9] = {
    {DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), 300},
    {DEG2DUTY(90), DEG2DUTY(50), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), 300},
    {DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(130), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), 300},
    {DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(50), 300},
    {DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(130), DEG2DUTY(90), DEG2DUTY(90), 300},
    {DEG2DUTY(90), DEG2DUTY(50), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), 300},
    {DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(130), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), 300},
    {DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(50), 300},
    {DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(130), DEG2DUTY(90), DEG2DUTY(90), 300},
    {DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), DEG2DUTY(90), 300},
};
float Dance2Steps[9][9] = {
    {DEG2DUTY(45), DEG2DUTY(70), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(70), 300},
    {DEG2DUTY(45), DEG2DUTY(115), DEG2DUTY(135), DEG2DUTY(65), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(70), 300},
    {DEG2DUTY(45), DEG2DUTY(70), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(65), DEG2DUTY(45), DEG2DUTY(115), 300},
    {DEG2DUTY(45), DEG2DUTY(115), DEG2DUTY(135), DEG2DUTY(65), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(70), 300},
    {DEG2DUTY(45), DEG2DUTY(70), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(65), DEG2DUTY(45), DEG2DUTY(115), 300},
    {DEG2DUTY(45), DEG2DUTY(115), DEG2DUTY(135), DEG2DUTY(65), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(70), 300},
    {DEG2DUTY(45), DEG2DUTY(70), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(65), DEG2DUTY(45), DEG2DUTY(115), 300},
    {DEG2DUTY(45), DEG2DUTY(115), DEG2DUTY(135), DEG2DUTY(65), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(70), 300},
    {DEG2DUTY(45), DEG2DUTY(75), DEG2DUTY(135), DEG2DUTY(105), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(70), 300},
};
float Dance3Steps[10][9] = {
    {DEG2DUTY(45), DEG2DUTY(70), DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(70), 300},
    {DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(60), DEG2DUTY(135), DEG2DUTY(70), DEG2DUTY(135), DEG2DUTY(70), 300},
    {DEG2DUTY(45), DEG2DUTY(70), DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(70), 300},
    {DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(70), DEG2DUTY(135), DEG2DUTY(120), 300},
    {DEG2DUTY(45), DEG2DUTY(70), DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(70), 300},
    {DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(60), DEG2DUTY(135), DEG2DUTY(70), DEG2DUTY(135), DEG2DUTY(70), 300},
    {DEG2DUTY(45), DEG2DUTY(70), DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(70), 300},
    {DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(70), DEG2DUTY(135), DEG2DUTY(120), 300},
    {DEG2DUTY(45), DEG2DUTY(70), DEG2DUTY(45), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(70), 300},
    {DEG2DUTY(90), DEG2DUTY(70), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(110), DEG2DUTY(90), DEG2DUTY(70), 300},
};

float StandSteps[1][9] = {
    {DEG2DUTY(45), DEG2DUTY(70), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(135), DEG2DUTY(110), DEG2DUTY(45), DEG2DUTY(70), 200},
};
float StorageSteps[1][9] = {
    {DEG2DUTY(135), DEG2DUTY(15), DEG2DUTY(45), DEG2DUTY(165), DEG2DUTY(45), DEG2DUTY(165), DEG2DUTY(135), DEG2DUTY(15), 200},
};
float SetSteps[1][9] = {
    {DEG2DUTY(130), DEG2DUTY(90), DEG2DUTY(50), DEG2DUTY(90), DEG2DUTY(50), DEG2DUTY(90), DEG2DUTY(130), DEG2DUTY(90), 200},
};

void sg90_init()
{
    mcpwm_config_t mcpwmConfig = {
        .frequency = 50,
        .cmpr_a = 0,
        .cmpr_b = 0,
        .counter_mode = MCPWM_UP_COUNTER,
    };
    mcpwmConfig.duty_mode = MCPWM_DUTY_MODE_0,
    mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0A, PIN_FOOT_1_1);
    mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0B, PIN_FOOT_1_2);
    mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM1A, PIN_FOOT_2_1);
    mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM1B, PIN_FOOT_2_2);
    mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM2A, PIN_FOOT_3_1);
    mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM2B, PIN_FOOT_3_2);
    mcpwm_gpio_init(MCPWM_UNIT_1, MCPWM0A, PIN_FOOT_4_1);
    mcpwm_gpio_init(MCPWM_UNIT_1, MCPWM0B, PIN_FOOT_4_2);
    mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_0, &mcpwmConfig);
    mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_1, &mcpwmConfig);
    mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_2, &mcpwmConfig);
    mcpwm_init(MCPWM_UNIT_1, MCPWM_TIMER_0, &mcpwmConfig);
    mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_GEN_A, DEG2DUTY(90));
    mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_GEN_B, DEG2DUTY(70));
    mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_GEN_A, DEG2DUTY(90));
    mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_GEN_B, DEG2DUTY(110));
    mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_GEN_A, DEG2DUTY(90));
    mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_GEN_B, DEG2DUTY(110));
    mcpwm_set_duty_in_us(MCPWM_UNIT_1, MCPWM_TIMER_0, MCPWM_GEN_A, DEG2DUTY(90));
    mcpwm_set_duty_in_us(MCPWM_UNIT_1, MCPWM_TIMER_0, MCPWM_GEN_B, DEG2DUTY(70));
}

void runSteps(float (*stepsArr)[9], int stepsCnt)
{
    for (int i = 0; i < stepsCnt; i++)
    {
        mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_GEN_A, stepsArr[i][0]);
        delay(STEP_INTERVAL);
        mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_GEN_B, stepsArr[i][1]);
        delay(STEP_INTERVAL);
        mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_GEN_A, stepsArr[i][2]);
        delay(STEP_INTERVAL);
        mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_GEN_B, stepsArr[i][3]);
        delay(STEP_INTERVAL);
        mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_GEN_A, stepsArr[i][4]);
        delay(STEP_INTERVAL);
        mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_GEN_B, stepsArr[i][5]);
        delay(STEP_INTERVAL);
        mcpwm_set_duty_in_us(MCPWM_UNIT_1, MCPWM_TIMER_0, MCPWM_GEN_A, stepsArr[i][6]);
        delay(STEP_INTERVAL);
        mcpwm_set_duty_in_us(MCPWM_UNIT_1, MCPWM_TIMER_0, MCPWM_GEN_B, stepsArr[i][7]);
        if (stepsArr[i][8] > STEP_INTERVAL * 7)
        {
            delay(stepsArr[i][8] - (STEP_INTERVAL * 7));
        }
        else
        {
            delay(STEP_INTERVAL);
        }
    }
}

void runStepsCmd(char *cmd){
    Serial.print("step:");
    Serial.println(cmd);
    if (!strcmp(cmd, "forward")){
        runSteps(ForwardSteps, sizeof(ForwardSteps) / sizeof(ForwardSteps[0]));
    }
    else if (!strcmp(cmd, "backward"))
    {
        runSteps(BackwardSteps, sizeof(BackwardSteps) / sizeof(BackwardSteps[0]));
    }
    else if (!strcmp(cmd, "moveLeft"))
    {
        runSteps(MoveLeftSteps, sizeof(MoveLeftSteps) / sizeof(MoveLeftSteps[0]));
    }
    else if (!strcmp(cmd, "moveRight"))
    {
        runSteps(MoveRightSteps, sizeof(MoveRightSteps) / sizeof(MoveRightSteps[0]));
    }
    else if (!strcmp(cmd, "turnLeft"))
    {
        runSteps(TurnLeftSteps, sizeof(TurnLeftSteps) / sizeof(TurnLeftSteps[0]));
    }
    else if (!strcmp(cmd, "turnRight"))
    {
        runSteps(TurnRightSteps, sizeof(TurnRightSteps) / sizeof(TurnRightSteps[0]));
    }
    else if (!strcmp(cmd, "lie"))
    {
        runSteps(LieSteps, sizeof(LieSteps) / sizeof(LieSteps[0]));
    }
    else if (!strcmp(cmd, "hello"))
    {
        runSteps(HelloSteps, sizeof(HelloSteps) / sizeof(HelloSteps[0]));
    }
    else if (!strcmp(cmd, "fight"))
    {
        runSteps(FightSteps, sizeof(FightSteps) / sizeof(FightSteps[0]));
    }
    else if (!strcmp(cmd, "pushUp"))
    {
        runSteps(PushUpSteps, sizeof(PushUpSteps) / sizeof(PushUpSteps[0]));
    }
    else if (!strcmp(cmd, "crabDance"))
    {
        runSteps(CrabDanceSteps, sizeof(CrabDanceSteps) / sizeof(CrabDanceSteps[0]));
    }
    else if (!strcmp(cmd, "dance1"))
    {
        runSteps(Dance1Steps, sizeof(Dance1Steps) / sizeof(Dance1Steps[0]));
    }
    else if (!strcmp(cmd, "dance2"))
    {
        runSteps(Dance2Steps, sizeof(Dance2Steps) / sizeof(Dance2Steps[0]));
    }
    else if (!strcmp(cmd, "dance3"))
    {
        runSteps(Dance3Steps, sizeof(Dance3Steps) / sizeof(Dance3Steps[0]));
    }
    else if (!strcmp(cmd, "stand"))
    {
        runSteps(StandSteps, sizeof(StandSteps) / sizeof(StandSteps[0]));
    }
    else if (!strcmp(cmd, "storage"))
    {
        runSteps(StorageSteps, sizeof(StorageSteps) / sizeof(StorageSteps[0]));
    }
    else if (!strcmp(cmd, "set"))
    {
        runSteps(SetSteps, sizeof(SetSteps) / sizeof(SetSteps[0]));
    }
    else
    {
        runSteps(StandSteps, sizeof(StandSteps) / sizeof(StandSteps[0]));
    }
}